#include "rclcpp/rclcpp.hpp"
#include "control_base/WMJRobotControl.h"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/float64.hpp"
#include <thread>
#include "control/WMJRobotControl.h"
#include "port/CanPort.hpp"
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::chrono_literals;
namespace wmj
{
    std::string gimbalControlPermissionToString(GIMBAL_CONTROL_PERMISSION permission)
    {
        switch (permission)
        {
        case GIMBAL_CONTROL_PERMISSION::AIMER:
            return "aimer";
        case GIMBAL_CONTROL_PERMISSION::SCAN:
            return "scan";
        default:
            return "default";
        }
    }
    //接受者的回调函数，直接把收到的信息发给了电控
    //需要使用的参数类型请看函数声明
    /**
     * @brief 使云台以最高速度转到目标角度
     *
     * @param base_interfaces::msg::GimbalPose msg. float msg.pitch俯仰角度,float msg.yaw  偏航角度
     */
    void Control::setGimbalAngleCallback(const base_interfaces::msg::GimbalPose::SharedPtr msg, GIMBAL_ORIENTATION orientation)
    {
        float pitch_angle = msg->pitch;
        float yaw_angle = msg->yaw;
        // RCLCPP_INFO(this->get_logger(), "gimbalControlPermissions:  %s", gimbalControlPermissionToString(m_gimbalControlPermissions).c_str());
        if (m_gimbalControlPermissions == msg->message_owner || m_gimbalControlPermissions == GIMBAL_CONTROL_PERMISSION::DEFAULT)
        {
            RobotControl->SetGimbalAngle(pitch_angle, yaw_angle, orientation);
        }
    }
    void Control::setGimbalAngleCallbackL(const base_interfaces::msg::GimbalPose::SharedPtr msg)
    {
        float pitch_angle = msg->pitch;
        float yaw_angle = msg->yaw;
        RCLCPP_INFO(this->get_logger(), "gimbalControlPermissions:  %s", gimbalControlPermissionToString(m_gimbalControlPermissions).c_str());
        if (m_gimbalControlPermissions == msg->message_owner || m_gimbalControlPermissions == GIMBAL_CONTROL_PERMISSION::DEFAULT)
        {
            RobotControl->SetGimbalAngle(pitch_angle, yaw_angle, LEFT);
        }
    }
    void Control::setGimbalAngleCallbackR(const base_interfaces::msg::GimbalPose::SharedPtr msg)
    {
        float pitch_angle = msg->pitch;
        float yaw_angle = msg->yaw;
        RCLCPP_INFO(this->get_logger(), "gimbalControlPermissions:  %s", gimbalControlPermissionToString(m_gimbalControlPermissions).c_str());
        if (m_gimbalControlPermissions == msg->message_owner || m_gimbalControlPermissions == GIMBAL_CONTROL_PERMISSION::DEFAULT)
        {
            RobotControl->SetGimbalAngle(pitch_angle, yaw_angle, RIGHT);
        }
    }
    /**
     * @brief 设定云台的角速度
     *
     * @param float msg.pitch 俯仰速度 float msg.yaw 偏航速度
     */
    void Control::setGimbalSpeedCallback(const base_interfaces::msg::GimbalPose::SharedPtr msg, GIMBAL_ORIENTATION orientation)
    {
        float pitch_speed = msg->pitch;
        float yaw_speed = msg->yaw;
        if (m_gimbalControlPermissions == msg->message_owner || m_gimbalControlPermissions == GIMBAL_CONTROL_PERMISSION::DEFAULT)
        {
            RobotControl->SetGimbalSpeed(pitch_speed, yaw_speed, orientation);
        }    
    }
    /**
     * @brief 设定云台的偏航角速度,俯仰角度
     *
     * @param float msg.pitch 俯仰角度 float msg.yaw 偏航速度
     */
    void Control::setGimbal_YawSpeed_PitchAngle_Callback(const base_interfaces::msg::GimbalPose::SharedPtr msg, GIMBAL_ORIENTATION orientation)
    {
        float pitch_angle = msg->pitch;
        float yaw_speed = msg->yaw;
        if (m_gimbalControlPermissions == msg->message_owner || m_gimbalControlPermissions == GIMBAL_CONTROL_PERMISSION::DEFAULT)
        {
            RobotControl->SetGimbal_YawSpeed_PitchAngle(pitch_angle, yaw_speed, orientation);
        }
    }
    /**
     * @brief 读取云台回传的角度数据
     *
     * @return base_interfaces::msg::GimbalPose GimbalPose；
     */
    void Control::publishGimbalAngle()
    {
        auto msg = base_interfaces::msg::GimbalPose();
        GimbalPose left_gimbal_data,right_gimbal_data;

        right_gimbal_data = RobotControl->GetGimbalAngle(RIGHT);
        left_gimbal_data = RobotControl->GetGimbalAngle(LEFT);

        // msg.pitch = gimbal_data.pitch;
        // msg.yaw = gimbal_data.yaw;
        // msg.roll = gimbal_data.roll;
        // msg.timestamp = gimbal_data.timestamp;

        sensor_msgs::msg::JointState joint_state;
        joint_state.header.stamp = this->now();
        joint_state.name.push_back("left_pitch_joint");
        joint_state.name.push_back("left_yaw_joint");
        joint_state.name.push_back("right_pitch_joint");
        joint_state.name.push_back("right_yaw_joint");
        joint_state.position.push_back(left_gimbal_data.pitch);
        joint_state.position.push_back(left_gimbal_data.yaw);
        joint_state.position.push_back(right_gimbal_data.pitch);
        joint_state.position.push_back(right_gimbal_data.yaw);
        // m_jointStatePublisher -> publish(joint_state);
        m_getGimbalAnglePublisher->publish(msg);
    }

    void Control::publishGimbalSpeed()
    {
        auto msg = base_interfaces::msg::GimbalPose();
        GimbalPose gimbal_data;

        gimbal_data = RobotControl->GetGimbalSpeed(LEFT);

        msg.pitch = gimbal_data.pitch;
        msg.yaw = gimbal_data.yaw;
        msg.roll = gimbal_data.roll;
        msg.timestamp = gimbal_data.timestamp;
        m_getGimbalSpeedPublisher->publish(msg);
    }
    /**
     * @brief 读取云台回传的角度数据，带有硬触发的
     *
     * @return base_interfaces::srv::GimbalPose msg;如果没收到请求返回的值均为非值
     */
    void Control::getGimbalAngleTriggerCallback(
        const base_interfaces::srv::GimbalPose::Request::SharedPtr request,
        const base_interfaces::srv::GimbalPose::Response::SharedPtr response)
    {
        if (request->request_msg == true)
        {
            GimbalPose OutputGimbalPose;
            int get_return;
            get_return = RobotControl->GetGimbalAngle(OutputGimbalPose);
            if (get_return == -1)
            {
                response->valid = -1;
                RCLCPP_WARN(this->get_logger(), "###########等待400us未获得IMU信息######!!!!!!!!!!!!!!!!!");
            }
            else
            {
                //收到请求，赋值
                response->pitch = OutputGimbalPose.pitch;
                response->yaw = OutputGimbalPose.yaw;
                response->roll = OutputGimbalPose.roll;
                response->timestamp = OutputGimbalPose.timestamp;
            }
        }
        else
        {
            //没收到请求，反馈一个非值
            RCLCPP_WARN(this->get_logger(), "#######未收到IMU请求########");
            char s = 'a';
            char *a = &s;
            response->pitch = std::nanf(a);
            response->yaw = std::nanf(a);
            response->yaw = std::nanf(a);
            response->timestamp = std::nan(a);
        }
    }
    /**
     * @brief 开启硬触发
     *
     * @return base_interfaces::srv::Trigger msg； 如果收到一次请求会一直返回1（只用开启一次，收到请求只会调用一次）
     * 如果开启失败想再次申请开启，请先请求0再请求1。
     */
    void Control::switchTriggerStateCallback(
        const base_interfaces::srv::Trigger::Request::SharedPtr request,
        const base_interfaces::srv::Trigger::Response::SharedPtr response)
    {
        static int n = 1;
        auto request_ = request->trigger_on;
        if (request_ != 0)
        {
            if (n == 1)
            {
                response->trigger_result = 1;
                RobotControl->CameraTrigger(true, 0, 100);
            }
            n++;
        }
        else
        {
            n = 1;
            std::cout << "没收到请求,此时的n:  " << n << std::endl;
        }
    }
    /**
     * @brief 获取扫描信息
     *
     * @return void
     *
     */
    void Control::scanSubscriptionCallback(const base_interfaces::msg::ScanCtrlInfo::SharedPtr msg)
    {
        if (msg->scan_mode != SCAN_MODE::SCAN_STOP)
            m_gimbalControlPermissions = GIMBAL_CONTROL_PERMISSION::DEFAULT;
    }

    /************************************新加的包************************/
    
    void Control::publishBulletSpeed(int pub_vec_index, GIMBAL_ORIENTATION orientation)
    {
        if(pub_vec_index < 0 || pub_vec_index > m_pub_vec.size()) {
            std::cout << "pub_vec_index value is illegal" << std::endl;
            return;
        }
        auto msg = std_msgs::msg::Float32();
        float bs = RobotControl->GetBulletSpeed(orientation);
        msg.data = bs;
        if (bs > 20 && bs != m_last_bullet_speed[orientation])
        {
            ((rclcpp::Publisher<std_msgs::msg::Float32>*)
            this->m_pub_vec[pub_vec_index].get())->publish(msg);
            std::cout<<"bullet_speed_bs: "<<bs<<std::endl;
            m_last_bullet_speed[orientation] = bs;
        }
    }
    void Control::publishGimbalAngle2(int pub_vec_index, GIMBAL_ORIENTATION orientation)
    {
        if(pub_vec_index < 0 || pub_vec_index > m_pub_vec.size()) {
            std::cout << "pub_vec_index value is illegal" << std::endl;
            return;
        }
        auto msg = base_interfaces::msg::GimbalPose();
        GimbalPose gimbal_data;

        gimbal_data = RobotControl->GetGimbalAngle(orientation);

        msg.pitch = gimbal_data.pitch;
        msg.yaw = gimbal_data.yaw;
        msg.roll = gimbal_data.roll;
        msg.timestamp = gimbal_data.timestamp;

        sensor_msgs::msg::JointState joint_state;
        joint_state.header.stamp = this->now();
        if(orientation == LEFT) 
        {
            joint_state.name.push_back("left_pitch_joint");
            joint_state.name.push_back("left_yaw_joint");
        }
        else
        {
            joint_state.name.push_back("right_pitch_joint");
            joint_state.name.push_back("right_yaw_joint");
        }
        joint_state.position.push_back(gimbal_data.pitch);
        joint_state.position.push_back(gimbal_data.yaw);

        m_jointStatePublisher->publish(joint_state);
        ((rclcpp::Publisher<base_interfaces::msg::GimbalPose>*)
        this->m_pub_vec[pub_vec_index].get())->publish(msg);
    }

    void Control::publishGimbalPoseAngleCallback(int pub_vec_index, GIMBAL_ORIENTATION orientation)
    {
        if(pub_vec_index < 0 || pub_vec_index > m_pub_vec.size()) {
            std::cout << "pub_vec_index value is illegal" << std::endl;
            return;
        }
        auto msg = base_interfaces::msg::GimbalPose();
        GimbalPose gimbal_data;

        gimbal_data = RobotControl->getGimbalPoseAngle(orientation);

        msg.pitch = gimbal_data.pitch;
        msg.yaw = gimbal_data.yaw;
        msg.roll = gimbal_data.roll;
        msg.timestamp = gimbal_data.timestamp;
        ((rclcpp::Publisher<base_interfaces::msg::GimbalPose>*)
        this->m_pub_vec[pub_vec_index].get())->publish(msg);
    }

    void Control::publishGimbalPoseSpeedCallback(int pub_vec_index, GIMBAL_ORIENTATION orientation)
    {
        if(pub_vec_index < 0 || pub_vec_index > m_pub_vec.size()) {
            std::cout << "pub_vec_index value is illegal" << std::endl;
            return;
        }
        auto msg = base_interfaces::msg::GimbalPose();
        GimbalPose gimbal_data;
        gimbal_data = RobotControl->getGimbalPoseSpeed(orientation);

        msg.pitch = gimbal_data.pitch;
        msg.yaw = gimbal_data.yaw;
        msg.roll = gimbal_data.roll;
        msg.timestamp = gimbal_data.timestamp;
        ((rclcpp::Publisher<base_interfaces::msg::GimbalPose>*)
        this->m_pub_vec[pub_vec_index].get())->publish(msg);
    } 

    void Control::publishGimbalRotationRangeCallback(int pub_vec_index, GIMBAL_ORIENTATION orientation)
    {
        if(pub_vec_index < 0 || pub_vec_index > m_pub_vec.size()) {
            std::cout << "pub_vec_index value is illegal" << std::endl;
            return;
        }
        auto data = RobotControl->getGimbalRotationRange(orientation);
        if(data.size() > 2)
        {
            auto msg = base_interfaces::msg::ScanCtrlInfo();
            msg.scan_mode = SCAN_MODE::SCAN_SET_RANGE;
            msg.start = data[0];
            msg.end = data[1];
            msg.scan_timestamp = wmj::now();
            ((rclcpp::Publisher<base_interfaces::msg::ScanCtrlInfo>*)
            this->m_pub_vec[pub_vec_index].get())->publish(msg);
        }
    }

    void Control::setBigYawCallback(const base_interfaces::msg::BigYaw::SharedPtr msg)
    {
        if(msg->mode & BigYawSpeed)
            RobotControl->setBigYaw(msg->mode, msg->speed);
        else
            RobotControl->setBigYaw(msg->mode, msg->angle);
    }

    void Control::BehaviorTreeCallBack(const base_interfaces::msg::BtJudge::SharedPtr msg)
    {
        if(msg->apply_bullet_redeem_num > (1 << 11) -1 || msg->apply_bullet_redeem_num < 0)
        {
            RCLCPP_ERROR(this->get_logger(),"SentryCmd2Judge: apply_bullet_redeem_num is out of range!");
            return;
        }
        if(msg->apply_remote_supply_times > (1 << 4) -1 || msg->apply_remote_supply_times < 0)
        {
            RCLCPP_ERROR(this->get_logger(),"SentryCmd2Judge: apply_remote_supply_times is out of range!");
            return;
        }
        if(msg->apply_remote_heal_times > (1 << 4) -1 || msg->apply_remote_heal_times < 0)
        {
            RCLCPP_ERROR(this->get_logger(),"SentryCmd2Judge: apply_remote_heal_times is out of range!");
            return;
        }

        if(msg->on_request) 
        {
            RobotControl->setSentryCmd(msg->comfirm_respawn, msg->apply_instant_respawn,
            msg->apply_bullet_redeem_num, msg->apply_remote_supply_times, msg->apply_remote_heal_times);
            RCLCPP_WARN(this->get_logger(),"SentryCmd2Judge: send SentryCmd!");
        }
    }



    void Control::publishBigYawCallback(int pub_vec_index)
    {
        if(pub_vec_index < 0 || pub_vec_index > m_pub_vec.size()) {
            std::cout << "pub_vec_index value is illegal" << std::endl;
            return;
        }
        base_interfaces::msg::BigYaw msg;
        std::vector<float> data = RobotControl->getBigYaw2();
        msg.mode = 0;
        msg.speed = data[3];
        msg.angle = data[1];

        sensor_msgs::msg::JointState joint_state;
        joint_state.header.stamp = this->now();
        joint_state.name.push_back("big_yaw_joint");
        joint_state.position.push_back(data[1]);

        m_jointStatePublisher->publish(joint_state);
        ((rclcpp::Publisher<base_interfaces::msg::BigYaw>*)
        this->m_pub_vec[pub_vec_index].get())->publish(msg);
    }


}